What to Expect from the Project

Initializing the Kalman Filter

As discussed previously, the process noise parameters have an important effect on your Kalman filter; you will need to tune the longitudinal and yaw acceleration noise parameters as part of the project.

The initial values for your state variables will also affect your Kalman filter's performance. Both the:

state vector x and the
state covariance matrix P

need to be initialized for the unscented Kalman filter to work properly.

Initializing the State Vector x

The state vector x contains x = [p_x, p_y, v, \psi, \dot{\psi}] .

You won't know where the bicycle is until you receive the first sensor measurement. Once the first sensor measurement arrives, you can initialize p_x and p_y .

For the other variables in the state vector x , you can try different initialization values to see what works best.

Note that although radar does include velocity information, the radar velocity and the CTRV velocity are not the same. Radar velocity is measured from the autonomous vehicle's perspective. If you drew a straight line from the vehicle to the bicycle, radar measures the velocity along that line.

In the CTRV model, the velocity is from the object's perspective, which in this case is the bicycle; the CTRV velocity is tangential to the circle along which the bicycle travels. Therefore, you cannot directly use the radar velocity measurement to initialize the state vector.

Initializing the State Covariance Matrix P

To initialize the state covariance matrix P , one option is to start with the identity matrix. For the CTRV model, P is a 5x5 matrix. The identity matrix would be:

P_{\text initial} = \begin{bmatrix} 1 \qquad 0 \qquad 0 \qquad 0 \qquad 0 \\ 0 \qquad 1 \qquad 0 \qquad 0 \qquad 0 \\ 0 \qquad 0 \qquad 1 \qquad 0 \qquad 0 \\ 0 \qquad 0 \qquad 0 \qquad 1 \qquad 0 \\ 0 \qquad 0 \qquad 0 \qquad 0 \qquad 1 \end{bmatrix}

Think back to what the state covariance matrix represents: take for example the value in the first row, second column. The value at (1, 2) would be the covariance \large \sigma_{px, py} measuring the linear relationship between the two variables.

The diagonal values represent the variances for each of the variables in the x state vector: \large [\sigma^2_{p_x}, \sigma^2_{p_y}, \sigma^2_{v}, \sigma^2_{\psi}, \sigma^2_{\dot{\psi}}] .

Why is the identity matrix a good place to start? Since the non-diagonal values represent the covariances between variables, the P matrix is symmetrical. The identity matrix is also symmetrical. The symmetry comes from the fact that the covariance between two variables is the same whether you look at (x, y) or (y, x): \large \sigma_{px, py} = \sigma_{py, px} . If you print out the P matrix in your UKF code, you will see that P remains symmetrical even after the matrix gets updated. If your solution is working, you will also see that P starts to converge to small values relatively quickly.

Instead of setting each of the diagonal values to 1, you can try setting the diagonal values by how much difference you expect between the true state and the initialized x state vector. For example, in the project, we assume the standard deviation of the lidar x and y measurements is 0.15. If we initialized p_x with a lidar measurement, the initial variance or uncertainty in
p_x would probably be less than 1.

You will have to experiment with different initialization values to find a working solution.